import rclpy
from rclpy.node import Node
from rclpy.clock import Clock
from motor_control_interfaces.msg import MotorCmd,MotorMsg
from geometry_msgs.msg import Twist,PointStamped
import math
import serial
from tf2_ros import TransformListener,Buffer
import tf2_ros
from rclpy.time import Time
import tf2_geometry_msgs

class ChassisController(Node):

    def __init__(self):

        super().__init__('chassis_controller')
       
        # ros话题设置
        self.motor_cmd_pub = self.create_publisher(MotorCmd, 'motor_cmd', 50)
        self.cmd_vel_sub = self.create_subscription(Twist, 'cmd_vel',self.cmd_vel_callback, 50)
        self.cmd_vel_sub
        
        # 定时器设置
        self.timer50ms = self.create_timer(0.1, self.timer50ms_callback)

        # 缓存
        self.last_speed=0
        self.last_theta=0

        # 当前状态：0刹车；10常规
        self.state=0
        
        # 上一帧的转角
        self.lastTheta=0.0
        
        # 导航控制
        self.nav_enable=False
        
        self.ser = serial.Serial(
            port='/dev/ttyUSB1',
            baudrate=100000,
            bytesize=8,
            parity='E',
            stopbits=2,
            timeout=1
        )
        
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
    


    def timer50ms_callback(self):
        remote_cmd=self.sbus_save(self.ser)
        # 接收机没数据或遥控器没数据或disable
        if remote_cmd is None or remote_cmd[0]==16 or remote_cmd[5]==1000:
            # 全部disable
            motor_cmd_msg=MotorCmd()
            motor_cmd_msg.header.stamp=self.get_clock().now().to_msg()
            for i in range(len(motor_cmd_msg.enable)):
                motor_cmd_msg.enable[i] = 0
            self.motor_cmd_pub.publish(motor_cmd_msg)
            self.nav_enable=False
            return
        # 下拉：遥控器模式
        if remote_cmd[5]==200:
            self.nav_enable=False
            # 自转模式
            if abs(remote_cmd[4])>20:
                self.brake_mode(remote_cmd[4])
            
            # 正常模式
            elif abs(remote_cmd[1])>20 or abs(remote_cmd[2])>20:
                speed,theta_degrees=self.cartesian_to_polar_constrained(remote_cmd[1]*2,-remote_cmd[2]*2)
                if theta_degrees-self.lastTheta>45:
                    theta_degrees=self.lastTheta+45
                if theta_degrees-self.lastTheta<-45:
                    theta_degrees=self.lastTheta-45
                if speed>1000:
                    speed==1000
                elif speed<-1000:
                    speed==-1000
                self.normal_mode(speed,theta_degrees*100000/360)
                self.lastTheta=theta_degrees
                
            else:
                self.normal_mode(0,self.lastTheta*100000/360)
            
                
            
        # 上拉：导航控制模式
        if remote_cmd[5]==1800:
            self.nav_enable=True
            return
        
    # 速度回调函数
    def cmd_vel_callback(self,msg):
        if self.nav_enable:
            try:
                transform = self.tf_buffer.lookup_transform(
                    "base_link","map",   # 目标坐标系和源坐标系
                    Time())
                point_msg=PointStamped()
                point_msg.point.x=msg.linear.x
                point_msg.point.y=msg.linear.y
                transform.transform.translation.x=0.0
                transform.transform.translation.y=0.0
                transform.transform.translation.z=0.0
                point_in_base_link = tf2_geometry_msgs.do_transform_point(point_msg, transform)
                v_x=point_in_base_link.point.y*500
                v_y=-point_in_base_link.point.x*500
            # 输出转化矩阵
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):    
                v_x=0.0
                v_y=0.0
            
            
            speed,theta_degrees=self.cartesian_to_polar_constrained(v_x,v_y)
            
            if speed==0:
                self.normal_mode(speed,self.last_theta)
            else:
                self.normal_mode(speed,theta_degrees*100000/360)
        
        
    # 常规模式
    def normal_mode(self,speed,theta):

        # 刹车转常规：等待1s让车轮复原，假设为10hz
        if self.state<30:
            self.state+=1
            speed=0
        # # 如果角度有跳变，停车
        # if abs(theta-self.last_theta)>45*100000/360:
        #     self.state=1
        #     speed=0
        # 更新角度
        self.last_theta=theta
        self.last_r=speed
        motor_cmd_msg=MotorCmd()
        motor_cmd_msg.header.stamp= self.get_clock().now().to_msg()
        # 全部使能
        for i in range(len(motor_cmd_msg.enable)):
            motor_cmd_msg.enable[i] = 1
        # 速度设置
        motor_cmd_msg.setvelocity[1] = int(speed)
        motor_cmd_msg.setvelocity[3] = int(speed)
        motor_cmd_msg.setvelocity[5] = int(speed)
        motor_cmd_msg.setvelocity[7] = int(speed)
        # 位置设置
        motor_cmd_msg.total_angle[0] = int(theta)
        motor_cmd_msg.total_angle[2] = int(theta)
        motor_cmd_msg.total_angle[4] = int(theta)
        motor_cmd_msg.total_angle[6] = int(theta)
        self.motor_cmd_pub.publish(motor_cmd_msg)

    # 刹车模式
    def brake_mode(self,angular_velocity):
        self.state=0
        motor_cmd_msg=MotorCmd()
        motor_cmd_msg.header.stamp=self.get_clock().now().to_msg()
        # 全部使能
        for i in range(len(motor_cmd_msg.enable)):
            motor_cmd_msg.enable[i] = 1
        # 位置设置
        motor_cmd_msg.total_angle[0] = -45*100000/360
        motor_cmd_msg.total_angle[2] = 45*100000/360
        motor_cmd_msg.total_angle[4] = -45*100000/360
        motor_cmd_msg.total_angle[6] = 45*100000/360
        # 速度设置
        motor_cmd_msg.setvelocity[1] = angular_velocity
        motor_cmd_msg.setvelocity[3] = -angular_velocity
        motor_cmd_msg.setvelocity[5] = -angular_velocity
        motor_cmd_msg.setvelocity[7] = angular_velocity
        self.motor_cmd_pub.publish(motor_cmd_msg)
        

    # 转到极坐标系下
    def cartesian_to_polar_constrained(self,x, y):
        # 计算电机速度
        r = math.sqrt(x**2 + y**2)
        theta_radians = math.atan2(y, x)
        theta_degrees = math.degrees(theta_radians)
        last_theta_=self.last_theta*360/100000
        # 限制在+-180
        if theta_degrees > 180:
            theta_degrees -= 360
        elif theta_degrees < -180:
            theta_degrees += 360

        if theta_degrees<=-90:
            theta_degrees+=180
            r=-r
        elif theta_degrees>=90:
            theta_degrees-=180
            r=-r
        
        # 震荡检测
        if abs(theta_degrees-last_theta_)>160:
            theta_degrees=last_theta_
            r=-self.last_r
        
        return r, theta_degrees
    
    # sbus解析
    def sbus_save(self,ser):
        
        # 尝试读取一个字节
        data=ser.read_until(expected=b'\x0F', size=100)
        # 通道列表
        channels_list=[]
        if data:  # 检测到帧头
            frame = ser.read(24)  # 读取剩余的24个字节
            if frame and frame[-1] == 0x00:  # 检查帧尾
                # 添加标志位
                channels_list.append(frame[22])
                
                # 解析第二个通道的数据（左手摇杆前后）
                data1 = frame[1]  # 第一个字节
                data2 = frame[2]  # 第二个字节
                channel_data = (data1>>3 |data2<<5)& 0x07FF
                channels_list.append(channel_data-1195)
                
                # 解析第四个通道的数据（左手摇杆左右）
                data1 = frame[4]  # 第五个字节
                data2 = frame[5]  # 第六个字节
                channel_data = ((data1 >> 1) | (data2 << 7))& 0x07FF  # 组合成11位的数据
                channels_list.append(channel_data-865)
                
                # 解析第三个通道的数据（右手摇杆前后）
                data1 = frame[2]  # 第一个字节
                data2 = frame[3]  # 第二个字节
                data3 = frame[4]  # 第三个字节
                channel_data = (data1>>6 |data2<<2 |data3<<10)& 0x07FF
                channels_list.append(channel_data)
                
                # 解析第一个通道的数据(右手摇杆左右)
                data1 = frame[0]  # 第一个字节
                data2 = frame[1]  # 第二个字节
                channel_data =   (data1|data2<<8) & 0x07FF  # 组合成11位的数据
                channels_list.append(channel_data-1185)
                
                # 解析第五个通道的数据（左开关）
                data1 = frame[5]  # 第五个字节
                data2 = frame[6]  # 第六个字节
                channel_data = ((data1 >> 4) | (data2 << 4))& 0x07FF  # 组合成11位的数据
                channels_list.append(channel_data)
                
                # 解析第六个通道的数据（右开关）
                data1 = frame[6]  # 第五个字节
                data2 = frame[7]  # 第六个字节
                data3 = frame[8]  # 第六个字节
                channel_data = ((data1 >> 7) | (data2 << 1) | (data3 << 9) )& 0x07FF  # 组合成11位的数据
                channels_list.append(channel_data)
                ser.flushInput()
                return channels_list
            else:
                # 如果帧不完整或帧尾不正确，清除输入缓冲区
                ser.flushInput()
        else:
            ser.flushInput()
        
        
def main(args=None):
    rclpy.init(args=args)
    chassis_controller = ChassisController()
    rclpy.spin(chassis_controller)
    chassis_controller.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
